!
! lsfit - least square fit
!
!
! Copyright © 2011 F.Hroch (hroch@physics.muni.cz)
!
! This file is part of Munipack.
!
! Munipack is free software: you can redistribute it and/or modify
! it under the terms of the GNU General Public License as published by
! the Free Software Foundation, either version 3 of the License, or
! (at your option) any later version.
! 
! Munipack is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
! GNU General Public License for more details.
! 
! You should have received a copy of the GNU General Public License
! along with Munipack.  If not, see <http://www.gnu.org/licenses/>.
!

module lsfit

  implicit none

  logical, parameter, private :: debug = .true.
  integer, parameter, private :: dbl = selected_real_kind(15)
  real(dbl), parameter, private :: rad = 57.295779513082322865_dbl
  real(dbl), parameter :: rtol = 1e-20
  real(dbl), dimension(:), allocatable, private :: x,y,u,v
  integer, private :: n
  real(dbl), private :: sres

  private :: lsminfun

contains

  subroutine lsminfun(np,p,fvec,iflag)
    
    integer, intent(in) :: np
    integer, intent(inout) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(np), intent(out) :: fvec
    real(dbl) :: sx,sy,sa,sb,rx,ry
    integer :: i
    
    if( iflag == 0 .and. debug ) write(*,'(a,4g15.3)') '#t:',p(1:np)

    sx = 0.0_dbl; sy = 0.0_dbl; sa = 0.0_dbl; sb = 0.0_dbl; sres = 0.0_dbl;
    do i = 1, n
       rx = u(i) - (p(1) + p(3)*x(i) - p(4)*y(i))
       ry = v(i) - (p(2) + p(4)*x(i) + p(3)*y(i))
       sx = sx - rx
       sy = sy - ry
       sa = sa - (rx*x(i) + ry*y(i))
       sb = sb - (rx*y(i) - ry*x(i))
       sres = sres + rx**2 + ry**2
!       write(*,'(9f10.5)') u(i),v(i),x(i),y(i),sx,sy,sa,sb
    end do
    fvec = (/sx,sy,sa,sb/)

    if( iflag == 0 .and. debug ) write(*,'(a,6g15.3)') '#fvec:',fvec(1:np),sum(fvec**2),sres

  end subroutine lsminfun


  subroutine lsmin(type,a,d,xc,yc,w,h,xx,yy,acen,dacen,dcen,ddcen,sc,dsc,pa,dpa,rms,s0)

    use astrotrafo
    use minpack

    ! parameters
    character(len=*),intent(in) :: type
    real(dbl),dimension(:),intent(in) :: a,d,xx,yy
    real(dbl),intent(in) :: xc,yc,w,h
    real(dbl),intent(in out) :: acen,dcen,sc,pa
    real(dbl),intent(out) :: dacen,ddcen,dsc,dpa,rms,s0

    ! fitting parameters
    integer, parameter :: npar = 4
    integer, parameter :: lwa = (npar*(3*npar+13))/2
    integer, parameter :: lr = (npar*(npar+1))/2
    real(dbl),dimension(npar) :: p,dp
    real(dbl),dimension(npar) :: fvec
    real(dbl),dimension(npar) :: diag,wa1,wa2,wa3,wa4,qtf
    real(dbl),dimension(npar,npar) :: fjac,cov
    real(dbl),dimension(lr) :: r
    integer,dimension(npar) :: ipvt
    integer :: info, nfev
    real(dbl) :: rp,rc,sc1,pa1,sc0

    ! auxiliaries
    integer :: i,j,m,it
    real(dbl), dimension(1) :: a0,d0
    type(TypeProjection) :: t


    if( size(a) < npar ) stop 'N=4 is absolute minimum of provided data.'

    if( debug ) then

       if( size(a) /= size(d) .or. size(a) /= size(xx) .or. size(xx) /= size(yy) ) stop 666
       
       if( w < 1 .or. h < 1.0 ) stop 667

    end if

    ! normalization
    n = size(a)
    allocate(x(n),y(n),u(n),v(n))
    sc0 = (w + h)/2.0_dbl
    x = (xx - xc)/sc0
    y = (yy - yc)/sc0

    ! initial projection
    t%type = type
    t%acen = acen
    t%dcen = dcen
    t%xcen = 0.0_dbl
    t%ycen = 0.0_dbl
    t%scale = 1.0_dbl
    t%pa = 0.0_dbl
    call trafo(t,a,d,u,v)

    ! fitting parameters
    p = (/0.0_dbl, 0.0_dbl, 1.0_dbl, 0.0_dbl/)
    sc1 = huge(sc1)
    pa1 = huge(pa1)

    do it = 1, 10
       if( debug ) write(*,*) '# iteration:',it

       ! transformation
       call hybrd(lsminfun,npar,p,fvec,rtol,200*(npar+1),npar-1,npar-1, &
            0.0_dbl, diag,1,100.0_dbl,1,info,nfev,fjac,npar, &
            r,lr,qtf,wa1,wa2,wa3,wa4)

       if( info == 0 ) stop 'Improper input parameters.'

       if( debug ) then
          write(*,*) '# par=',p
          write(*,*) '# info=',info
          write(*,*) '# fjac=',(fjac(i,:),i=1,size(fjac,1))
          write(*,*) '# r=',r
          write(*,*) '# qtf=',qtf
          write(*,*) '# fvec=',fvec
       end if

       call invtrafo(t,(/p(1)/),(/p(2)/),a0,d0)
       
       acen = a0(1)
       dcen = d0(1)
       sc = sc0/sqrt(p(3)**2 + p(4)**2)
       pa = rad*atan2(p(4),p(3))
       s0 = sres

       rp = pa - pa1
       rc = sc - sc1

       write(*,'(a,en12.3,en13.3,f8.1,2f14.5)') "=LSFIT> ",s0,sc,pa,acen,dcen

       ! covariance matrix
       cov = 0
       m = 1
       do j = 1,npar
          do i = j,npar
             cov(i,j) = r(m)
             cov(j,i) = r(m)
             m = m + 1
          end do
       end do
       
       ipvt = (/ (i,i=1,npar) /)
       call covar(npar,cov,npar,ipvt,epsilon(cov),wa1)
       if( debug ) then
          write(*,*) '# inverse:'
          write(*,*) '# ',(cov(i,1:npar),i=1,npar)
       end if

       ! uncertainities
       do i = 1,npar
          dp(i) = sqrt(s0*cov(i,i)/(n - npar))
       enddo

       ! output parameters with deviations
       dsc = sc - sc0/sqrt((p(3) + dp(3))**2 + (p(4) + dp(4))**2)
       dacen = dp(1)
       ddcen = dp(2)
       rms = sqrt(s0/(n - npar))
       dpa = abs(atan2(p(4),p(3)) - atan2(p(4)+dp(4),p(3)+dp(3))) + &
            abs(atan2(p(4),p(3)) - atan2(p(4)-dp(4),p(3)+dp(3))) + &
            abs(atan2(p(4),p(3)) - atan2(p(4)-dp(4),p(3)-dp(3))) + &
            abs(atan2(p(4),p(3)) - atan2(p(4)+dp(4),p(3)-dp(3)))
       dpa = rad*dpa/4.0

       ! finish when parameters are small
       if( it > 1 .and. abs(p(1)) < dp(1) .and. abs(p(2)) < dp(2) .and. &
            abs(rp) < dpa .and. abs(rc) < dsc ) exit

       ! new projection by using of fitted parameters
       t%acen = acen
       t%dcen = dcen
       call trafo(t,a,d,u,v)

    end do

    if( debug ) then
       write(*,*) '# solution:',p
       write(*,*) '# uncerts.: ',dp
       write(*,*) '# diag:  ',diag
       write(*,*) '# scale: ',sc,dsc
       write(*,*) '# s0: ',s0,sum(fvec**2)
    end if

    deallocate(x,y,u,v)

  end subroutine lsmin

end module lsfit

